# vrpn.py - hacked up version that reads in data through vrpn_print_devices

import re, time, os

class vrpn :
    def __init__(self, target, host="localhost:4321") :
        self.host = host
        cmd = [ "vrpn_print_devices" ]
        if isinstance(target, ( list, tuple )) :
            cmd.extend(target)
        elif isinstance(target, dict) :
            cmd.extend( target.keys() )
        else :
            cmd.append(target)
        
        import subprocess
        print "Executing %s" % " ".join(cmd)
        self.pipe = subprocess.Popen(cmd, stdout=subprocess.PIPE)
        self.queue = []
        self.dev = None
        self.sensor = None
    
    def __del__(self) :
        os.kill(self.pipe.pid, 1)
        time.sleep(0.1)
        # Close pipes
        for h in [ self.pipe.stdout, self.pipe.stdin, self.pipe.stderr ] :
            if h : h.close()
        # If still alive, kill it
        if self.is_alive() :
            os.kill(self.pipe.pid, 9)
            print "Killing VRPN"
        print "VRPN pipe closed"
    
    def is_alive(self) :
        return self.pipe.poll() == None
    
    def pop(self, n = -1) :
        if len(self.queue) == 0 :
            return None
        return self.queue.pop(n)
    
    def populate(self) :
        # Parse out all of the lines coming off the pipe
        import select
        rlist, wlist, elist = select.select([ self.pipe.stdout ], [], [], 0.005)
        if len(rlist) :
            line = self.pipe.stdout.readline()
            self.parse_line(line)
    
    def parse_line(self, line) :
        if re.search(
            r"No \ response | VRPN \ (?: Warning | Error)", line, re.X
        ) : return
        
        if re.match(r"^ Analog", line, re.X) :
            self.dev = re.match(r"^ Analog \s+ (\S+@\S+):", line, re.X).group(1)
            return
        if re.match(r"^ \s+ -? \d+ \. \d+", line, re.X) :
            self.queue.append({
                "mode" : "analog",
                "dev"  : self.dev,
                "data" : [
                    float(x) for x in
                    re.findall("( -? \d+ \. \d+ )", line, re.X)
                ],
                "sensor" : None,
            })
            return
        if re.match(r"^ Button", line, re.X) :
            self.dev = re.match(r"^ Button \s+ (\S+@\S+),", line, re.X).group(1)
            m = re.search(r"number (\d+) was just (\w+)", line)
            self.queue.append({
                "mode" : "button",
                "dev"  : self.dev,
                "data" : [
                    float( m.group(1) ),
                    m.group(2),
                ],
                "sensor" : None,
            })
            return
        m_tracker = re.match(r"""
            ^ Tracker \s+ (\S+@\S+), \s+ sensor\ (\d+):
        """, line, re.X)
        if m_tracker :
            # Tracker Tracker0@localhost:4321, sensor 2:
            self.dev = m_tracker.group(1)
            self.sensor = m_tracker.group(2)
        if re.search(r"\bpos\b", line) and re.search(r"\bquat\b", line) :
            m = re.findall(r"(-? \d+ \. \d+)", line, re.X)
            self.queue.append({
                "mode" : "tracker",
                "dev"  : self.dev,
                "data" : {
                    "pos" : map(float, m[0:3]),
                    "quat" : map(float, [ m[6], m[3], m[5], m[4] ]),
                },
                "sensor" : int(self.sensor),
            })
            return

